#include <STDLIB.H>
#include "Int_Motor.h"
#include "Dri_Grip.h"
#include "Dri_Timer_2.h"
#include "Int_OLED_Data.h"

/** 左边速度 */
static char S_L_SPEED = 0;
/** 左边方向 1= 前进, 0 = 后退 */
static bit S_L_DIRECTION = 0;

/** 右边速度 */
static char S_R_SPEED = 0;
/** 右边方向 1= 前进, 0 = 后退 */
static bit S_R_DIRECTION = 0;

/**
 * @brief 初始化,马达
 *
 */
void Int_Motor_Init()
{
    // 开启供电
    MOTOR_EN = 1;
    // 电机开启
    MOTOR_A1 = 0;
    MOTOR_A2 = 0;
    MOTOR_B1 = 0;
    MOTOR_B2 = 0;
    Dri_Timer_AddCallBack(Int_Motor_Timer2_Back);
}

/**
 * @brief 添加左边速度
 *
 * @param speed 速度
 */
void Int_Motor_setLeft(char speed)
{
    if (speed >= 40) {
        speed = 40;
    }
    if (speed <= -40) {
        speed = -40;
    }

    // 判断方向
    S_L_DIRECTION = (speed > 0) ? 1 : 0;
    // 数据绝对值
    S_L_SPEED = abs(speed);
}

/**
 * @brief 添加右边速度
 *
 * @param speed 速度
 */
void Int_Motor_setRight(char speed)
{
    if (speed >= 40) {
        speed = 40;
    }
    if (speed <= -40) {
        speed = -40;
    }

    S_R_DIRECTION = (speed > 0) ? 1 : 0;
    // 方向绝对值
    S_R_SPEED = abs(speed);
}

/**
 * @brief timer2的回调函数
 *
 */
void Int_Motor_Timer2_Back()
{
    // // 算占空率,已40为一个周期
    static u8 s_timer2_count = 0;

    // 大于则高电平,这个周期高电平的占比
    if (S_L_SPEED > s_timer2_count) {
        MOTOR_A1 = S_L_DIRECTION;
        MOTOR_A2 = ~S_L_DIRECTION;
    } else {
        MOTOR_A1 = 0;
        MOTOR_A2 = 0;
    }

    // // 电机马达有镜像,所以需要反过来
    if (S_R_SPEED > s_timer2_count) {
        MOTOR_B2 = S_R_DIRECTION;
        MOTOR_B1 = ~S_R_DIRECTION;
    } else {
        MOTOR_B1 = 0;
        MOTOR_B2 = 0;
    }

    if (s_timer2_count >= 40) {
        s_timer2_count = 0;
    }
    s_timer2_count++;
}